#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "test_interfaces/action/progress.hpp"
#include "lifecycle_msgs/srv/change_state.hpp" // 包含生命周期服务中的更改状态服务


using test_interfaces::action::Progress;
using GoalHandleProgress = rclcpp_action::ClientGoalHandle<Progress>;
using namespace std::placeholders;

class LifecycleActionClient : public rclcpp::Node
{

public:
    explicit LifecycleActionClient(const rclcpp::NodeOptions &node_options = rclcpp::NodeOptions())
        : Node("LifecycleActionClient", node_options)
    {
        // 创建动作客户端
        client_ptr_ = rclcpp_action::create_client<Progress>(this, "get_sum");
        lifecycle_control_client_ = this->create_client<lifecycle_msgs::srv::ChangeState>("lifecycle_action_server/change_state");
    }

    // 发送请求
    // 发送目标请求，send_goal 函数发送目标给服务器，目标值是num，传递给服务器用于求和
    void send_goal(int64_t num);

    /// @brief 切换生命周期节点状态
    /// @param transition_id 配置的状态：e.g: lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE
    void change_state(uint8_t transition_id);

private:
    // 动作通信客户端指针
    rclcpp_action::Client<Progress>::SharedPtr client_ptr_;
    // 生命周期节点控制节点
    rclcpp::Client<lifecycle_msgs::srv::ChangeState>::SharedPtr lifecycle_control_client_;

    // 生命周期客户端指针

    // == 处理响应 ==
    // 1. 处理目标发送后的反馈——服务器收到请求时调用，确定是否接受目标
    void goal_response_callback(GoalHandleProgress::SharedPtr goal_handle);

    // 2. 处理连续反馈——收到服务器的进度反馈时调用，显示百分比进度
    void feedback_callback(GoalHandleProgress::SharedPtr, const std::shared_ptr<const Progress::Feedback> feedback);

    // 3. 处理最终响应——当服务器完成任务时调用，显示最终求和结果
    // WrappedResult 是 ROS2 Action的一个类型，用于封装一个操作(action)的最终结果，包括成功与失败等状态信息
    void result_callback(const GoalHandleProgress::WrappedResult &result);
};

void LifecycleActionClient::send_goal(int64_t num)
{
    if (!this->client_ptr_)
    {
        RCLCPP_ERROR(this->get_logger(), "动作客户端未被初始化 ");
    }

    // 等待服务器响应: wait_for_action_server 确保客户端可以连接到服务器
    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10)))
    {
        RCLCPP_INFO(this->get_logger(), "服务连接失败！");
        return;
    }

    auto goal_msg = Progress::Goal();
    goal_msg.num = num;
    RCLCPP_INFO(this->get_logger(), "发送请求数据！");

    // 这行代码创建了一个 SendGoalOptions对象，用于配置和管理发送动作目标请求时的选项和回调i函数
    auto send_goal_options = rclcpp_action::Client<Progress>::SendGoalOptions();
    // 1. 目标发送后的响应（服务器接收到目标后的反馈）
    send_goal_options.goal_response_callback = std::bind(&LifecycleActionClient::goal_response_callback, this, _1);
    // 2. 处理服务器发送的连续进度反馈 服务器调用publish_feedback方法发送数据时触发
    send_goal_options.feedback_callback = std::bind(&LifecycleActionClient::feedback_callback, this, _1, _2);
    // 3. 处理动作执行完毕时的最终结果 服务器调用 succeed(), abort(), canceled() 方法发送数据时触发，传递最终结果
    send_goal_options.result_callback = std::bind(&LifecycleActionClient::result_callback, this, _1);
    // 通过ROS2动作客户端异步发送一个目标(goal)请求，并返回一个future对象，用于异步等待服务器的响应
    auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
    /** 关键点
     *  1. 异步发送目标
     *     - 该方法是异步的，不会立即等待服务器的响应，而是继续执行后面的代码。服务器接收目标的响应将在回调函数(如 goal_response_callback)中处理
     *  2. future对象
     *     - goal_handle_future 是一个future，它允许你在稍后使用get()来获取目标的处理状态，或者通过回调函数处理目标的执行和反馈
     * */
}

void LifecycleActionClient::change_state(uint8_t transition_id)
{
    // 检查服务是否可用
    if(!lifecycle_control_client_->wait_for_service(std::chrono::seconds(3)))
    {
        RCLCPP_ERROR(this->get_logger(), "Service not available after waiting");
        return;
    }

    // 创建请求并设置目标转换
    auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
    request->transition.id = transition_id;

    // 异步发送请求
    auto result = lifecycle_control_client_->async_send_request(request);

    // 等待结果
    if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS)
    {
        RCLCPP_INFO(this->get_logger(), "State change successful!");
    }
    else
    {
        RCLCPP_INFO(this->get_logger(), "Failed to change state");
    }
}

void LifecycleActionClient::goal_response_callback(GoalHandleProgress::SharedPtr goal_handle)
{
    if (!goal_handle)
    {
        RCLCPP_ERROR(this->get_logger(), "目标请求被服务器拒绝！");
    }
    else
    {
        RCLCPP_INFO(this->get_logger(), "目标被接收，等待结果中");
    }
}

void LifecycleActionClient::feedback_callback(GoalHandleProgress::SharedPtr, const std::shared_ptr<const Progress::Feedback> feedback)
{
    int32_t progress = (int32_t)(feedback->progress * 100);
    RCLCPP_INFO(this->get_logger(), "当前进度： %d%%", progress);
}

void LifecycleActionClient::result_callback(const GoalHandleProgress::WrappedResult &result)
{
    switch (result.code)
    {
    case rclcpp_action::ResultCode::SUCCEEDED:
        break;
    case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "任务被中止");
        return;
    case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "任务被取消");
        return;
    default:
        RCLCPP_ERROR(this->get_logger(), "未知异常");
        return;
    }
    RCLCPP_INFO(this->get_logger(), "任务执行完毕，最终结果：%ld", result.result->sum);
}

int main(int argc, char** argv)
{
    // 初始化 ROS2 客户端
    rclcpp::init(argc, argv);

    // 调用spin函数，并传入节点对象指针
    auto action_client = std::make_shared<LifecycleActionClient>();    // 创建对象并发送请求

    // 发送切换状态信息
    action_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); // 切换到config
    action_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); // 切换到activate

    // 发送请求
    action_client->send_goal(10);

    // rclcpp::spin(action_client);    // 使用rclcpp::spin保持节点运行，持续监听和处理来自订阅、服务、动作、定时器等的回调函数
    rclcpp::shutdown();
    return 0;
}
